//
// Created by hoyin on 2022/4/13.
//
#ifndef MAIN_CUDA_HPP
#define MAIN_CUDA_HPP

#include <iostream>
#include <chrono>
#include "opencv2/opencv.hpp"
#include "MVCamera.h"
#include "SingleTrack.h"
#include "Matcher.h"
#include "UndistortImage.h"
#include "CvUtils.h"
#include "ImageStream.h"
#include "thread"
#include "Chrono.h"
#include "Filter.h"
#include "Optimizer.h"
#include "MathUtils.h"

using namespace st;
using namespace cv;
using namespace mu;
using namespace opt;

//#define FRAME_WIDTH 678
//#define FRAME_HEIGHT 512

#define RETURN_VALUE_SUCCESS 0
#define RETURN_VALUE_ERROR 1
#define RETURN_VALUE_ILLEGAL_COMMAND 2
#define RETURN_VALUE_UNDEFINED_TYPE_OF_STREAM 3

// 图像尺寸
#define FRAME_WIDTH 640
#define FRAME_HEIGHT 368

#define PI 3.14159265

// 帧间隔
#define FRAME_TIME_SPAN 42
// 飞行时间 + 机动时间 + 其他时间
#define TIME_SPAN 0.5
// 直线距离
#define STRAIGHT_DISTANCE 10
// 重力加速度
#define G 9.8
// 装甲板在图像中的像素
#define ARMOUR_WIDTH_PIX 35.86666463157895
#define ARMOUR_HEIGHT_PIX 24.679530213903742
// 装甲板的实际长度
#define ARMOUR_WIDTH_ACT 14.5329608467882
#define ARMOUR_HEIGHT_ACT 10
// 线性尺度倍数 实际长度(m)/像素个数(pix)
#define SCALE 0.4051940986448067
// 绘图框数量
#define CANVAS_COUNT 4


int main_cuda(ImageStream &ims) {
//	初始化匹配器
	Mat centerTemplate = imread("./media/centerTemplate.png", IMREAD_GRAYSCALE);
	cuda::GpuMat gCenterTemplate;
	gCenterTemplate.upload(centerTemplate);
	Matcher matcher(gCenterTemplate);
	matcher.setGpuAccelMode(true);
	matcher.setTargetImage(gCenterTemplate);
//	等待100ms跳过无效帧
	std::this_thread::sleep_for(std::chrono::milliseconds(100));
//	时间戳
	double ts;
//	定义原始帧
	Mat frame;
//	中心点图像
	Mat centerImg;
	cuda::GpuMat gFrame;
	cuda::GpuMat gFrameBin;
	cuda::GpuMat gFrameBinFlipped;
	cuda::GpuMat gCenterImg;

//	R标中心点
	Rect2d rLogoCenterRect;
	Moments m;
	Point2d rLogoPoint;

//	未打击扇叶
	Mat armourTemplate, armourTemplateBin;
//	cuda::GpuMat gArmourTemplate, gArmourTemplateBin;
//	cuda::GpuMat gFanNotHit;
	Rect2d fanNotHitRect;

//	未打击装甲板
	RotatedRect armourNotHitRect_rotate;
	Point2d armourNotHitCenter;

//	未打击装甲板的中心点
	Point2f armourCenterPoint_img;	// 图像坐标系下
	Point2d armourCenterPoint_cal;	// 计算坐标系下

//	偏转角
	Vec2d vecArmour(0, 0);	// 装甲板位置向量
	Vec2d vecX_p(1, 0);	// x轴正向单位向量

//	上一R标位置，用于防止位置差太大的误匹配
	Point2d prevRLogoPoint(-1, -1);


//	形态学卷积核
	Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));//		形态学滤波器
	Ptr<cuda::Filter> filterMRClose = cuda::createMorphologyFilter(MORPH_CLOSE, CV_8UC1, element);

//	散点绘图帧、绘图数据流
	Mat canvas[CANVAS_COUNT];
	for (Mat &c: canvas) {
		c = Mat(FRAME_HEIGHT, FRAME_WIDTH, CV_8UC3, Scalar(255, 255, 255));
	}
	vector<double> data_x[CANVAS_COUNT];
	vector<double> data_y[CANVAS_COUNT];
//	上一角度，用于计算角度差
	double prev_angle = 0;
//	上一时间戳，用于计算时间差
	double prev_ts = 0;

//	滤波器、结果流、原始流
	Filter filter(5);
	vector<vector<double>> filterResultStream;
	vector<vector<double>> unfilteredStream;
//	数据记录流
	ofstream originLogOFS("./logs/velocity");
	ofstream filteredLogOFS("./logs/filteredVelocity");
	ofstream sizeOFS("./logs/rectSize");
	if (!originLogOFS.is_open()) {
		cout << "open origin log failed" << endl;
	}

	if (!filteredLogOFS.is_open()) {
		cout << "open filtered log failed" << endl;
	}
//	优化器初始采样数、优化参数表、优化数据流
	int optN = 20;
	double ae = 0.785, we = 0.884, te = 0.0, ye = 1.305;
	double params[4] = {ae, we, te, ye};
	vector<double> opt_data_x, opt_data_y;
	Optimizer optimizer(params, optN);
//	预测角度、半径、直角坐标
	double esti_theta = 0;
	double R = 0;
	Point2d estiPoint_img, estiPoint_cal;
//	Yaw, Pitch, Roll, v 参数
	double yaw, pitch, roll = 0, v;
	double z = STRAIGHT_DISTANCE;
//	计时器
	Chrono chrono;
//	开始
	while (ims.isOK()) {
//		chrono.reset();
//		时间戳
		ts = 0;
		ims.read(frame, ts);
//		原视频有些影响匹配的
		if (frame.empty()) {
			continue;
		}
		for (int c = 0; c < 200; ++c) {
			for (int r = 300; r < 368; ++r) {
				frame.at<Vec3b>(r, c)[0] = 0;
				frame.at<Vec3b>(r, c)[1] = 0;
				frame.at<Vec3b>(r, c)[2] = 0;
			}
		}
		for (int c = 310; c < 325; ++c) {
			for (int r = 165; r < 180; ++r) {
				frame.at<Vec3b>(r, c)[0] = 20;
				frame.at<Vec3b>(r, c)[1] = 20;
				frame.at<Vec3b>(r, c)[2] = 200;
			}
		}
////		原始图像帧上传到GPU
//		cuda::GpuMat gFrame;
//		cuda::GpuMat gFrameBin;
//		cuda::GpuMat gFrameBinFlipped;
//		cuda::GpuMat gCenterImg;

		gFrame.upload(frame);
		flip(frame, frame, 0);

//		取二值图
		cuda::cvtColor(gFrame, gFrameBin, COLOR_BGR2GRAY);
		cuda::threshold(gFrameBin, gFrameBin, 50, 255, THRESH_BINARY);

//		翻转图像
		cuda::flip(gFrameBin, gFrameBinFlipped, 0);
		gFrameBin = gFrameBinFlipped;
//		cout << "ts_0：" << 1000*chrono.timeSpan() << endl;
//		闭运算
		filterMRClose->apply(gFrameBin, gFrameBin);

//		查找R标中心点
		matcher.setOriginImage(gFrameBin);
		matcher.contoursMatch(rLogoCenterRect);
		if (rLogoCenterRect.empty()) {
			continue;
		}
		gCenterImg = gFrameBin(rLogoCenterRect);
		gCenterImg.download(centerImg);
		m = moments(centerImg, true);
		rLogoPoint.x = m.m10 / m.m00 + rLogoCenterRect.x;
		rLogoPoint.y = m.m01 / m.m00 + rLogoCenterRect.y;
//		防止位置差距太大造成误匹配
		if (prevRLogoPoint.x == -1 && prevRLogoPoint.y == -1) {
			prevRLogoPoint.x = rLogoPoint.x;
			prevRLogoPoint.y = rLogoPoint.y;
		} else {
			if (norm(prevRLogoPoint - rLogoPoint) > 10) {
				rLogoPoint.x = prevRLogoPoint.x;
				rLogoPoint.y = prevRLogoPoint.y;
			}
		}
		circle(frame, rLogoPoint, 2, Scalar(200, 20, 20), 2);
//		cout << "ts_1：" << 1000*chrono.timeSpan() << endl;

//		查找未打击扇叶
		cuda::GpuMat gArmourTemplate, gArmourTemplateBin;
		cuda::GpuMat gFanNotHit;

		armourTemplate = imread(FAN_TEMPLATE, IMREAD_GRAYSCALE);
		gArmourTemplate.upload(armourTemplate);
		cuda::threshold(gArmourTemplate, gArmourTemplateBin, BINARY_THRESHOLD, 255, THRESH_BINARY);
		filterMRClose->apply(gArmourTemplateBin, gArmourTemplateBin);

		matcher.setTargetImage(gArmourTemplateBin);
		matcher.contoursMatch(fanNotHitRect);
		if (fanNotHitRect.empty()) {
			continue;
		}
		gFanNotHit = gFrameBin(fanNotHitRect);
//		cout << "ts_2：" << 1000*chrono.timeSpan() << endl;

//		查找未打击装甲板
		armourTemplate = imread(ARMOUR_TEMPLATE, IMREAD_GRAYSCALE);
		gArmourTemplate.upload(armourTemplate);
		cuda::threshold(gArmourTemplate, gArmourTemplateBin, BINARY_THRESHOLD, 255, THRESH_BINARY);
		filterMRClose->apply(gArmourTemplateBin, gArmourTemplateBin);
		matcher.setTargetImage(gArmourTemplateBin);
		matcher.setOriginImage(gFanNotHit);
		matcher.contoursMatch(armourNotHitRect_rotate);

		if (armourNotHitRect_rotate.size.empty()) {
			continue;
		}
		armourNotHitCenter = armourNotHitRect_rotate.center;
		armourNotHitCenter.x += fanNotHitRect.x;
		armourNotHitCenter.y += fanNotHitRect.y;
		armourNotHitRect_rotate.center = armourNotHitCenter;
//		cout << "ts_3：" << 1000*chrono.timeSpan() << endl;

/**
 * 		输出装甲板在相机中的尺寸，用于计算尺度常数
		double l1 = armourNotHitRect_rotate.size.width;
		double l2 = armourNotHitRect_rotate.size.height;
		if (l1 > l2) {
			sizeOFS << l1 << " " << l2 << endl;
		} else {
			sizeOFS << l2 << " " << l1 << endl;
		}
*/
//		查找未打击装甲板的中心点
		drawRotatedRect(armourNotHitRect_rotate, frame, Scalar(20, 200, 20), 2, Point2d(0, 0));
		getCenterOfRect(armourNotHitRect_rotate, armourCenterPoint_img);
		circle(frame, armourCenterPoint_img, 2, Scalar(200, 200, 20), 2);
//		未打击装甲板中心点和R标的连线
		line(frame, rLogoPoint, armourCenterPoint_img, Scalar(200, 20, 20));

//		计算半径
		R = sqrt(pow(armourCenterPoint_img.x - rLogoPoint.x, 2) + pow(rLogoPoint.y - armourCenterPoint_img.y, 2));

//		获取偏转角
		convertPos_Image2Calc(armourCenterPoint_img, rLogoPoint, armourCenterPoint_cal);
		vecArmour[0] = armourCenterPoint_cal.x;
		vecArmour[1] = armourCenterPoint_cal.y;
		double angle = getAngleOf2Vectors(vecArmour, vecX_p);
		if (getQuadrant(vecArmour) > 2) {
			angle = 2 * PI - angle;
		}
//		计算瞬时速度
		double velocity = (angle - prev_angle) / (ts - prev_ts);
//		为下一循环设置变量
		prev_angle = angle;
		prev_ts = ts;
//		cout << "ts_4：" << 1000*chrono.timeSpan() << endl;

/**
		输出数据流到文件
		originLogOFS << ts << " " << velocity << " " << angle << endl;
*/

//		输出数据到原始数据流
		if (!isnan(velocity)) {
			unfilteredStream.push_back({ts, velocity});
//			进行滤波
			if (filter.readyToFilter()) {
				vector<vector<double>> filteredV = Filter::classifyFilter(unfilteredStream, filter.getFlag() -	filter.getCapacity(), filter.getFlag(), 0.25, 2.5);
//				滤波结果加入结果流和优化器输入流
				if (!filteredV.empty()) {
					for (auto &fv: filteredV) {
						filterResultStream.push_back(fv);
						opt_data_x.push_back(fv.at(0));
						opt_data_y.push_back(fv.at(1));
						optimizer.increaseFlag();
/**
//						输出滤波结果到文件
						filteredLogOFS << fv.at(0) << " " << fv.at(1) << endl;
*/
					}
				}
			}
			filter.updateFlag();
//			非线性优化
			if (optimizer.readyToOptimize()) {
				optimizer.solveTrigonometric(opt_data_x, opt_data_y);
				double d_angle = integrateSin(params[0], params[1], params[2], params[3], ts, ts + TIME_SPAN);
				esti_theta = angle + d_angle;
				estiPoint_cal.x = R * cos(esti_theta);
				estiPoint_cal.y = R * sin(esti_theta);
			}
		}
//		cout << "ts_5：" << 1000*chrono.timeSpan() << endl;

/**
//		速度函数优化结果
//		double optvelocity = params[0]* sin(params[1]*(ts + params[2])) + params[3];
*/
/**
//		输出数据到散点图
		data_x[0].push_back(ts);
		data_y[0].push_back(angle);
		drawPlot(data_x[0], data_y[0], canvas[0], Scalar(123, 104, 238), 0, 0, -10, 10);
		imshow("angle", canvas[0]);
		data_x[1].push_back(ts + TIME_SPAN);
		data_y[1].push_back(esti_theta);
		drawPlot(data_x[1], data_y[1], canvas[1], Scalar(84, 139, 84), 0, 0, -10, 10);
		imshow("esti_angle", canvas[1]);
		data_x[2].push_back(ts + TIME_SPAN);
		data_y[2].push_back(yaw);
		drawPlot(data_x[2], data_y[2], canvas[2], Scalar(238, 99, 99), 0, 0, -1, 1);
		imshow("yaw", canvas[2]);
		data_x[3].push_back(ts + TIME_SPAN);
		data_y[3].push_back(pitch);
		drawPlot(data_x[3], data_y[3], canvas[3], Scalar(139, 35, 35), 0, 0, -3, 3);
		imshow("pitch", canvas[3]);
*/

//		显示预测点位置
		convertPos_Calc2Image(estiPoint_cal, rLogoPoint, estiPoint_img);
		circle(frame, estiPoint_img, 3, Scalar(20, 200, 200), 3);
//		计算ypr参数
		const double x = armourCenterPoint_cal.x * SCALE;
		const double y = armourCenterPoint_cal.y * SCALE;
		const double Sy = sqrt(x*x + z*z);

		yaw = atan(z / Sy);

		double y_ = 0.5*y + 0.25*G*TIME_SPAN*TIME_SPAN;
		double s_ = sqrt(x*x + pow(y - y_, 2) + z*z);
		double rho_ = acos((y - y_) / s_);

		v = Sy / (TIME_SPAN * sin(rho_));
		pitch = rho_;

//		显示ypr参数
		putText(frame, format("yaw: %f rad, pitch: %f rad, speed: %f m/s", yaw, pitch, v), Point(20, 40), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(20, 200, 200), 2);
//		显示追踪结果
		putText(frame, format("ts: %f s", ts), Point(200, 60), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 255), 2);
		putText(frame, format("a: %f deg", angle * 180 / 3.14159), Point(20, 60), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 255), 2);
//		显示单帧耗时
//		putText(frame, format("time_cost: %f ms", 1000*chrono.timeSpan()), Point(20, 80), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 255), 2);
		cout << format("time_cost: %f ms", 1000*chrono.timeSpan()) << endl;
		imshow("frame", frame);
		waitKey(1);
//		cout << "ts_6：" << 1000*chrono.timeSpan() << endl << endl;

	}

	return RETURN_VALUE_SUCCESS;
}

#endif